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Spacecraft  Constrained  Maneuver  Planning  Using  Positively  Invariant 

Constraint  Admissible  Sets  (Postprint) 

Avishai  Weiss,  Morgan  Baldwin,  R.  Scott  Erwin  and  Ilya  Kolmanovsky 


Abstract — The  paper  considers  spacecraft  motion  planning 
based  on  the  use  of  safe  positively  invariant  sets.  In  this 
approach,  a  connectivity  graph  is  constructed  between  a  set 
of  forced  equilibria,  forming  a  virtual  net  that  is  centered 
around  a  nominal  orbital  position.  The  connectivity  between 
two  equilibria  is  determined  based  on  safe  positively  invariant 
sets  in  order  to  guarantee  that  transitions  between  equilibria 
can  be  effected  while  spacecraft  actuator  limits  are  adhered 
to  and  debris  collisions  are  avoided.  A  graph  search  algorithm 
is  implemented  to  find  the  shortest  path  around  the  debris. 
Simulation  results  are  presented  that  illustrate  this  approach. 

I.  Introduction 

With  the  growing  amount  of  debris  in  Earth  orbit,  space¬ 
craft  maneuver  planning  procedures  have  to  address  debris 
avoidance  requirements.  While  obstacle  avoidance  is  a  stan¬ 
dard  problem  in  robotics  [2],  [3],  the  related  spacecraft 
problems  have  several  unique  features.  In  particular,  the 
space  environment  is  relatively  uncluttered,  thus  permitting 
for  a  variety  of  maneuvers.  Spacecraft  dynamics  are  quite 
different  from  those  of  typical  robots.  Maneuver  efficiency 
with  respect  to  time  and  fuel  consumption  is  a  critical 
consideration.  The  states  of  the  spacecraft  and  the  debris  can 
only  be  estimated,  often  with  a  significant  estimation  error. 
Finally,  computational  algorithms  must  be  fast  and  optimized 
given  moving  objects  and  the  limited  computing  power  on¬ 
board  most  spacecraft.  These  unique  features  of  spacecraft 
maneuver  planning  problems  provide  the  motivation  for  the 
development  of  specialized  algorithms. 

In  [1],  we  have  introduced  an  on-board  maneuver  planning 
approach  based  on  the  use  of  constraint-admissible  positively 
invariant  sets  to  determine  connectivity  between  a  set  of 
forced  and  unforced  spacecraft  equilibria  forming  a  virtual 
net  in  the  vicinity  of  the  spacecraft.  Two  equilibria  are  con¬ 
nected  if  a  choice  of  a  Linear  Quadratic  (LQ)  feedback  gain 
can  be  made  that  results  in  a  transition  between  the  equilibria 
which  avoids  the  debris  collision  and  satisfies  the  limits  on 
thrust.  The  connectivity  graph  for  all  the  equilibria  in  the 
net  is  constructed  and  real-time  graph  search  algorithms  are 
used  to  optimize  an  equilibria  hopping  sequence  that  avoids 
the  debris  collisions. 

A.  Weiss  is  a  Graduate  Student,  Department  of  Aerospace  Engineering, 
The  University  of  Michigan,  Ann  Arbor,  ML 

M.  Baldwin  is  a  Research  Aerospace  Engineer,  Space  Vehicles  Direc¬ 
torate,  Air  Force  Research  Laboratory,  Kirtland  Air  Force  Base,  NM. 

R.  Scott  Erwin  is  a  Principal  Research  Aerospace  Engineer,  Space 
Vehicles  Directorate,  Air  Force  Research  Laboratory,  Kirtland  Air  Force 
Base,  NM. 

I.  Kolmanovsky  is  a  Professor,  Department  of  Aerospace  Engineering, 
The  University  of  Michigan,  Ann  Arbor,  MI. 


Unlike  the  open-loop  trajectory  optimization  approaches, 
we  do  not  rely  on  precise  assignment  of  spacecraft  position 
to  the  time  instants  along  the  trajectory,  but  instead  switch  to 
the  next  set-point  and  controller  gain  once  appropriate  con¬ 
ditions  are  satisfied.  While  this  approach  is  conservative,  it 
facilitates  fault-tolerant  and  disturbance-tolerant  execution  of 
the  maneuvers.  Furthermore,  by  using  disturbance-invariant 
sets  [13]  in  the  construction,  we  can  assure  robustness  to 
unmeasured  (but  set-bounded)  disturbances  and  uncertain¬ 
ties.  This  extension  to  handling  unmeasured  disturbances  and 
uncertainties  using  techniques  of  [13]  is  not  pursued  here. 

To  facilitate  the  on-board  computations  of  the  connec¬ 
tivity  graph,  a  fast  growth  distance  computation  procedure 
between  two  ellipsoidal  sets  has  been  proposed  in  [1].  In 
this  approach,  using  the  Karush-Kuhn-Tucker  conditions,  the 
growth  distance  computations  are  reduced  to  a  root  finding 
problem  for  the  scalar  value  of  the  Lagrange  multiplier.  Then 
a  predictor-corrector  dynamic  Newton-Raphson  algorithm 
is  used  to  update  the  Lagrange  multiplier  thereby  rapidly 
estimating  the  growth  distance  from  different  equilibria  in 
the  virtual  net  to  the  debris. 

In  this  paper,  we  incorporate  limited  thrust  requirements 
into  the  computation  of  thrust  limit  on  the  growth  distance, 
and  we  simulate  maneuvers  that  adhere  to  the  limited  thrust 
constraints.  Even  though  the  computation  of  thrust  limits 
on  the  growth  distance  can  be  performed  offline  for  the 
nominal  operating  conditions,  fast  computational  procedures 
are  beneficial  in  case  of  thruster  failures,  degradations,  and 
restrictions  on  thrust  directions  (e.g.,  caused  by  the  presence 
of  other  spacecraft  nearby),  all  of  which  can  lead  to  changing 
constraints  on  thrust  during  spacecraft  missions.  If  the  thrust 
limits  are  prescribed  in  the  form  of  2 -norm  bounds,  the 
optimization  problem  involved  in  computing  the  thrust  limit 
on  the  growth  distance  is  non-convex  (unlike  computing  the 
growth  distance  itself).  Consequently,  we  advocate  the  use 
of  polyhedral  norm  bounds  on  thrust,  that  lend  themselves 
to  explicit  solutions. 

The  related  literature  on  spacecraft  trajectory  optimization 
with  obstacle/debris  avoidance  is  surveyed  in  [1].  Previous 
research  addresses  topics  in  spacecraft  trajectory  optimiza¬ 
tion  [5],  collision  avoidance  strategies  based  on  risk  as¬ 
sessment  [6],  the  use  of  artificial  potential  functions  [7], 
[8],  and  the  use  of  conventional  and  mixed  integer  linear 
programming  techniques  [9],  [10],  [11],  [12]. 

The  paper  is  organized  as  follows.  In  Section  II  we  discuss 
the  nonlinear  and  the  linearized  models  used  to  represent 
spacecraft  relative  motion  dynamics.  In  Section  III  we  review 
our  approach  to  constructing  the  virtual  net  based  on  a  set 
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of  forced  and  unforced  equilibria  and  using  this  virtual  net 
for  debris  avoidance.  The  procedure  to  compute  the  thrust 
limit  on  the  growth  distance  is  presented.  Simulation  results 
are  reported  in  Section  IV.  Finally,  concluding  remarks  are 
made  in  Section  V. 

II.  Relative  Motion  Model 

The  spacecraft  relative  motion  model  represents  the  space¬ 
craft  dynamics  in  the  (non-inertial)  Hill’s  frame  with  the 
origin  at  a  target  location  on  a  nominal  circular  orbit.  For 
small  distances,  the  linearization  of  the  relative  motion  model 
given  by  the  Hill-Clohessy- Wiltshire  (CWH)  equations  is 
used  [14]. 

A.  Nonlinear  equations  of  motion 

The  relative  position  vector  of  the  spacecraft  with  respect 
to  a  target  location  on  a  circular  orbit  is  expressed  as 

Sr  =  xi  +  yi  +  zfc, 

where  x,  y  and  z  are  the  components  of  the  position  vector 
of  the  spacecraft  relative  to  the  target  location  and  i,  %  k  are 
the  unit  vectors  of  the  Hill’s  frame.  The  Hill’s  frame  has  its 
x-axis  along  the  orbital  radius,  y- axis  along  the  orbital  track, 
and  z-axis  is  orthogonal  to  orbital  plane. 

The  position  vector  of  the  spacecraft  with  respect  to  the 
center  of  the  Earth  can  be  expressed  as  R  =  To  +  Sr  = 
(Ro  +  x)i  +  yj  +  zfc,  where  Rq  is  the  nominal  orbital  radius. 
The  nonlinear  equations  of  motion  for  the  spacecraft  (relative 
to  an  inertial  frame)  can  be  expressed  in  vector  form  as 

n=~^  +  —F,  (i) 

R 6  mc 

where  F  is  the  vector  of  external  forces  applied  to  the 
spacecraft,  R  =  \R\,  mc  is  the  mass  of  the  spacecraft,  y 
is  the  gravitational  constant,  and 

R  =  (x  —  2  ny  —  n2x)i  +  (y  +  2  nx  —  n2y)j  +  (z)k. 

In  these  equations,  n  =  denotes  the  mean  motion  of 

the  nominal  orbit.  Similar  equations  can  be  used  to  describe 
the  motion  of  the  debris. 

B.  Linearized  HCW  equations  in  discrete-time 

For  5r  «  R ,  the  linearized  Hill-Clohessy- Wiltshire 
(HCW)  equations  [14]  approximate  the  relative  motion  of 
the  spacecraft  on  a  circular  orbit  as 

9  Fx 

x  —  3  n2x  —  2ny  =  — , 
mc 

Fy 

y  +  2nx  = — ,  (2) 

mc 

2  Fz 

z  +  n  z  =  — , 
mc 

where  Fx,Fy,  Fz  are  components  of  the  external  force  vector 
(excluding  gravity)  acting  on  the  spacecraft.  The  linearized 
dynamics  account  for  differences  in  gravity  between  the 
spacecraft  and  nominal  orbital  location,  and  for  relative 
motion  effects.  The  spacecraft  relative  motion  dynamics  in 


the  orbital  plane  (x  and  y)  and  in  the  out-of-orbital  plane 
(z)  are  decoupled.  The  in-plane  dynamics  are  Lyapunov 
unstable,  while  the  out-of-plane  dynamics  are  Lyapunov 
stable.  The  in-plane  dynamics  are  completely  controllable 
from  Fy  input  but  are  not  controllable  from  Fx  input.  The 
out-of-plane  dynamics  are  controllable  from  Fz  input.  These 
dynamics  are  thus  different  from  typical  robots. 

Assuming  a  sampling  period  of  AT  sec,  we  can  convert 
the  model  (2)  to  a  discrete-time  form 

A(f+.l)  =  AX(t)  +  BU(t),  (3) 

where  X{t)  =  [x(t),  y{t),  z(t),  x(t),  y(t),z(t)}T  is 

the  state  vector  at  the  time  instant  t  G  Z+,  U(t)  = 
[Fx (t) ,  Fy(t ),  Fz(t)]T  is  the  control  vector  of  thrust  forces 
at  the  time  instant  t  G  Z+,  and  A  =  exp(AcAT ),  B  = 
JQAT  exp(Ac(AT  —  r))drBc  are  the  discretized  matrices 
obtained  based  on  the  continuous -time  system  realization 
(. AC1BC )  in  (2).  Alternatively,  the  control  vector  V  can 
represent  an  instantaneous  change  in  the  velocity  of  the 
spacecraft,  Av,  induced  by  thrust,  with  an  appropriately  re¬ 
defined  T-matrix, 

0  0  " 

0  0 
0  0 
0  0  ' 

1  0 
0  1  _ 

III.  Debris  Avoidance  Based  on  a  Virtual  Net 

Our  approach  to  debris  avoidance  is  based  on  utilizing 
constraint-admissible  positively  invariant  sets  [13],  [15]  cen¬ 
tered  around  the  spacecraft  forced  and  unforced  equilibria. 
A  finite  set  of  these  equilibria  used  for  constructing  debris 
avoidance  maneuvers  is  referred  to  as  a  virtual  net.  Given 
an  estimate  of  the  debris  position,  we  build  a  connectivity 
graph  that  identifies  the  equilibria  in  the  virtual  net  between 
which  the  spacecraft  can  move,  with  guaranteed  collision- 
free  motion  and  within  the  available  thrust  authority.  We  then 
employ  graph  search  to  determine  an  efficient  path  between 
the  equilibria  that  ensures  debris  avoidance. 

A.  Virtual  Net 

The  virtual  net  comprises  a  finite  set  of  equilibria,  Xe(r), 
corresponding  to  a  finite  set  of  prescribed  spacecraft  posi¬ 
tions  r  G  M  =  {ri,r2, . . .  ,rn}  C  R 3, 


Xe(rk)  = 


whose  velocity  states  are  zero,  and  where  n  is  the  number 
of  equilibria  in  the  virtual  net.  See  Figure  1 .  We  assume  that 
for  all  r  G  Af,  the  corresponding  values  of  control  necessary 
to  support  the  specified  equilibria  in  steady- state  satisfy  the 
imposed  thrust  limits. 


T'kx 

rky 

Tkz 

0 

0 

0 


k  =  1,  •  ■ 


(4) 


Bav  =  e 


Acat 
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Fig.  1 :  The  virtual  net  for  debris  avoidance.  Dots  correspond 
to  positions  at  equilibria,  Xe(r),  on  a  virtual  net.  The 
ellipsoid  represents  the  debris  position  and  uncertainty. 


B.  LQ  Controller  with  Gain  Switching 

A  conventional  Linear- Quadratic  (LQ)  feedback  is  used  to 
control  the  spacecraft  to  a  commanded  equilibrium  in  (4), 

U  =  K{X  -  Xe(r ))  +  Tr  =  KX  +  H(K)r ,  (5) 


where 


T  = 


—3  n2mc  0  0 

0  0  0 
0  0  n2mc 


H(K)  =  Y  —  K  ^  , 

and  where  Is  denotes  the  3  x  3  identity  matrix  and  O3 
denotes  the  3x3  zero  matrix.  This  LQ  controller  provides 
an  asymptotically  stable  closed-loop  system  but  does  not 
enforce  the  constraints. 

To  provide  greater  flexibility  in  handling  constraints,  a 
multimode  controller  architecture  is  employed  [15].  Specif¬ 
ically,  we  assume  that  a  finite  set  of  LQ  gains  K  G  JC  = 
{Ki,-—  ,  ATm}  is  available  to  control  the  spacecraft.  By 
using  a  large  control  weight  in  the  LQ  cost  functional, 
motions  with  low  fuel  consumption  yet  large  excursions  can 
be  generated;  using  a  large  control  weight  in  the  LQ  cost, 
motions  with  short  transition  time  can  be  generated  [16].  We 
assume  that  a  preference  ordering  has  been  defined  and  the 
gains  are  arranged  in  the  order  of  descending  preference, 
from  K\  being  the  highest  preference  gain  to  Km  being  the 
lowest  preference  gain. 


C.  Positively  Invariant  Sets 
The  ellipsoidal  set 

C(r,K)  = 

{XeR6:  ±(X  -  Xe(r))TP(K)(X  -  Xe(r))  <  1}  C  R6, 

(6) 

where  (A  +  BK)TP(A  +  BK)  -  P  <  0,  P  =  P{K)  >  0, 
is  positively  invariant.  Positive  invariance  implies  that  any 
trajectory  of  the  closed-loop  system  that  starts  in  C(r,K)  is 
guaranteed  to  stay  in  C(r,K)  as  long  as  the  same  LQ  gain 
K  is  used  and  the  set-point  command  r  is  maintained.  To 


achieve  the  positive  invariance,  the  matrix  P  can  be  obtained 
as  the  solution  of  the  discrete-time  Riccati  equation  or  of  the 
above  Lyapunov  equation  for  the  closed-loop  asymptotically 
stable  system.  We  note  that,  because  the  system  is  linear,  the 
positive  invariance  of  C(r,K )  implies  the  positive  invariance 
of  the  scaled  set 

C(r,  K,p)  =  {XeR6:^(X-  Xe(r))T P(K)(X  -  Xe(r)) 
<  P2},  P>  o. 

Geometrically,  the  set  C  (r,  if,  p)  corresponds  to  an  ellipsoid 
scaled  by  the  value  of  p  and  centered  around  Xe{r),  r  G  M. 


D.  Debris  Representation 

We  use  a  set,  0(z,Q),  centered  around  the  position  z  G 
R 3,  to  over-bound  the  position  of  the  debris,  i.e., 

0(z,  Q)  =  {X  e  R6  :  ( SX  -  z)tQ(SX  -  z)  <  1},  (7) 


where  Q  =  QT  >  0  and 


S  = 


1  0  0  0  0  0 
0  1  0  0  0  0 
0  0  1  0  0  0 


(8) 


The  set  0(z,Q)  can  account  for  the  debris  and  spacecraft 
physical  sizes  and  also  for  the  uncertainties  in  the  estimation 
of  the  debris/spacecraft  position.  Note  that  the  set  0(z,Q) 
has  an  ellipsoidal  shape  in  the  position  directions  and  it 
is  unbounded  in  the  velocity  directions.  Ellipsoidal  sets 
rather  than  polyhedral  sets  are  used  to  over-bound  the  debris 
since  ellipsoidal  bounds  are  typically  produced  by  position 
estimation  algorithms,  such  as  the  Extended  Kalman  Filter 
(EKF). 


E.  Debris  Avoidance  Approach 

Consider  now  G  A /*,  representing  a  possible  position 
on  the  net  that  the  spacecraft  can  move  to  as  a  part  of  the 
debris  avoidance  maneuver.  Suppose  that  the  current  state  of 
the  spacecraft  is  X(to)  at  the  time  instant  to  £  ^+-  If  there 
exists  a  p  >  0  and  Kj  e  /C  such  that 

X(t0)  e  C(ri,Kj,p)  and  0(z,Q)  r\C{ri,Kj,p)  =  0,  (9) 

the  spacecraft  can  move  to  the  position  G  AT  by  engaging 
the  control  law  with  r(t)  =  ri  and  K(t )  =  Kj,  t  >  to, 
and  without  hitting  the  debris  confined  to  0(z,Q).  This 
idea  underlies  our  subsequent  approach  to  debris  avoidance, 
where  we  maintain  the  spacecraft  trajectories  within  the  tube 
formed  by  positively  invariant  sets  that  do  not  overlap  with 
the  debris. 

To  avoid  a  non- stationary  debris,  its  path  can  be  covered 
by  a  union  of  a  finite  number  of  sets  of  ellipsoidal  shape, 

l—nd 

U  0(zt,Qt),  (10) 

1  =  1 

where  the  center  of  the  Zth  set  is  denoted  by  zi  G  R 3  and  the 
Zth  set  shape  is  defined  by  Qi  =  Qj  >  0.  Then  the  debris 
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path  avoidance  condition  is 

X(to)  €  C{?i,  Kj,p)  and  0(zt,Qi )  n  C{n,Kj,p)  =  0, 

for  all  /  =  1,  •  •  •  ,  nd • 

(ID 

The  same  approach,  with  larger  nd,  can  be  used  to  handle 
multiple  non-stationary  debris.  Note,  however,  that  this  ap¬ 
proach  is  conservative  as  it  does  not  account  for  the  debris 
progressions  along  their  paths  versus  time. 

F.  Growth  Distances 

The  minimum  value  of  p  >  0  for  which 
0(z,  Q)  P|  C(r,  K,  p)  7^  0  is  referred  to  as  the  growth 
distance,  similarly  to  [17].  This  growth  distance  can  also 
be  viewed  as  the  least  upper  bound  on  the  values  of  p 
for  which  0(z,Q)  and  C(r,K,p)  do  not  intersect.  See 
Figure  2.  We  use  the  notation  pg(r,  K,Q,  z)  to  reflect  the 
dependence  of  the  growth  distance  on  the  set-point  r  G  A/*, 
the  control  gain  K  G  JC  and  the  obstacle  parameters  Q  and 
z. 

Note  that  the  growth  distance  depends  on  the  position 
of  the  debris  which  may  be  unknown  in  advance.  Con¬ 
sequently,  the  growth  distance  computations  have  to  be 
performed  online.  The  computations  of  the  growth  distance, 
p9(r,  K,Q,z\  have  been  addressed  in  [1];  for  completeness, 
these  computations  are  summarized  in  this  paper. 


Fig.  2:  The  positively  invariant  set  is  grown  till  touching  the 
debris.  The  spacecraft  can  move  from  any  of  the  equilibria  on 
the  virtual  net  inside  the  positively  invariant  set  C(r,  iT,  p) 
to  Xe{ri)  marked  by  ’x’  without  colliding  with  the  debris. 

Since  the  spacecraft  maneuvers  have  to  be  performed  using 
limited  thrust,  we  additionally  define  a  maximum  value  of 
p  =  pw(r,  K )  for  which  I  G  C(r,  P,  pn(r,  K))  implies  that 
the  thrust  U  =  KX  +  H(K)r  satisfies  the  imposed  thrust 
limits.  We  refer  to  pu  as  the  thrust  limit  on  growth  distance. 
Unlike  pg,  the  value  of  pu  does  not  depend  on  the  position 
or  shape  of  the  debris  and  can  be  pre-computed  off-line. 

Finally,  we  define  the  thrust  limited  growth  distance 

p*(r,K,Q,z)  =  min {pg(r,  K,Q,  z),  pu(r,  K)}.  (12) 

Note  that  X(to)  G  C(r, ,  Kj ,  (r, ,  Kj ,  z) )  implies  that  the 
ensuing  closed-loop  spacecraft  trajectory  under  the  control 
(5)  with  r(t)  =  Vi  and  K(t)  =  Kj  for  £  >  £0  satisfies  the 


thrust  limits  and  avoids  debris  collisions  for  a  debris  confined 
to  0(z,  Q ). 

The  above  definitions  were  given  for  the  case  of  a  single 
stationary  debris,  0(z,  Q ).  In  the  case  of  multiple  debris  or 
when  avoiding  a  predicted  debris  path  based  on  (10),  the 
growth  distance  is  replaced  by  the  multi-growth  distance , 
which  is  the  minimum  growth  distance  to  each  of  0(zi,Qi ), 
/  =  1,  •  •  •  ,nd. 

G.  Growth  Distance  Computations 

Define  X  =  X  —  Xe(r)  and  a  =  2  p2 .  The  problem  of 
determining  the  growth  distance  pg(r,K,Q,z),  reduces  to 
the  following  constrained  optimization  problem: 

min  a 

a,X 

subject  to 

XTPX  <  a, 

((S(X  +  Xe(r))  -  z)tQ((S(X  +  Xe(r))  -  z)  <  1. 

(13) 

To  solve  this  optimization  problem,  we  use  the  Karush- 
Kuhn-Tucker  (KKT)  conditions.  Note  that  the  standard  linear 
independence  constraint  qualification  conditions  hold  given 
that  P  >  0.  We  define 

C  =  a  +  \1(XTPX  -a) 

+A 2((S(X  +  Xe{r))  -  z)TQ(S(X  +  Xe{r))  -  z)  -  1), 

where  Ai  and  A2  are  Lagrange  multipliers.  The  stationarity 
of  the  Lagrangian  (setting  partial  derivative  equal  to  zero) 
with  respect  to  a  yields  Ai  =  1.  The  stationarity  of  the 
Lagrangian  with  respect  to  X  leads  to 

X  =  X(X2,  r,  z )  =  -(P+X2STQS)-1STQ{SXe{r)-z)X2, 

(14) 

where  A2  >  0  is  a  scalar  to  be  determined.  Note  that  P  >  0, 
STQS  >  0,  A2  >  0  (as  the  Lagrange  multiplier  correspond¬ 
ing  to  an  inequality  constraint)  imply  that  (P  +  A2  STQS) 
is  invertible.  The  problem  reduces  to  finding  a  nonnegative 
scalar  A 2  which  is  the  root  of  the  equation 

F(A2,  r,  z)  =  ((SX  -  z)tQ(SX  -  z)  -  1  =  0,  (15) 

where 

X  =  X(A2,  r,  z)  +  Xe(r). 

The  scalar  root  finding  problem  (15)  has  to  be  solved 
online  multiple  times  for  different  r  G  A/",  and  in  the  case 
of  avoiding  a  predicted  debris  path  also  for  different  z’s.  To 
solve  this  problem  fast  and  to  re-use  the  previously  found 
solutions  as  approximations,  in  [1]  we  have  proposed  a 
dynamic  Newton-Raphson’s  algorithm.  This  algorithm  uses 
predictor-corrector  updates  to  track  the  root  as  a  function  of 
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z  and  r: 


A^+1’+  =  Xk  +  {  —  (Xk,zk,rk)}-1{-F(Xk,zk,rk) 

-  ^(Xk,zk,rk)(zk+1-zk) 

-  ^(X  k,zk,rk)(rk+1-rk)}, 

Xk2+1  =  max{0,X2+1’+}. 

(16) 

To  implement  the  algorithm,  we  take  advantage  of  known 
functional  form  for  F  and  compute  explicitly  the  partial 
derivatives, 


Fig.  3:  Components  of  r,  rx,  ry  and  rz  varying  versus  the 
iteration  number. 


=  (P  +  X2StQS)~1  {- STQ(SXe(r )  -  z) 

OA2 

- stqsx }  , 

^  =  (p  +  x  2stqs)~1  {-sTQsn}  x2, 

^-=2(SX-z  +  rfQ(Sd^  +  I3), 


dX 

dz 


(. P  +  X2STQS)~1STQSnX2 , 


Fig.  4:  Growth  distance  versus  iteration  number  computed 
by  dynamic  Newton-Raphson  algorithm. 


-  =  2  {SX-z  +  rfQ(S--I3). 

In  these  equations, 

Xe(r)  =  where  O  =  ^  , 

and  Is  denotes  the  3  x  3  identity  matrix.  Note  that  SQ  =  I3. 

Figures  3-5  illustrate  the  growth  distance  tracking  in  the 
case  when  z  =  [0,0, 0]T,  Q  =  4 J3,  and  P  =  PT  >  0  is 
determined  as  a  solution  to  the  Lyapunov  equation, 


P =s 
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For  the  first  20  iterations,  rk  is  a  constant  to  enable  initial 
convergence  of  the  algorithm  to  take  place.  Then  rk  starts 
to  vary  through  the  virtual  net,  see  Figure  3.  One  iteration 
of  the  Newton-Raphson  algorithm  per  value  of  rk  is  used 
to  update  the  root,  A^1.  Figure  4  demonstrates  that  the 
growth  distance  tracking  is  accurate.  The  growth  distance 
occasionally  becomes  zero  indicating  the  overlap  between 
the  debris  and  several  of  rk.  This  is  confirmed  from  Figure  5, 
which  illustrates  the  trajectory  of  rk  in  the  three  dimensional 
Hills’  frame  relative  to  the  debris. 


H.  Thrust  Limit  on  Growth  Distance  Computations 

In  [1],  the  following  procedure  to  compute  pu(r,K )  has 
been  discussed.  Suppose  that  the  thrust  limits  are  expressed 
in  the  form  |  \LU\  \  <  1  for  an  appropriately  defined  matrix  L 
and  norm  1 1  •  1 1 .  The  computational  procedures  to  determine 
pu(r,K)  involves  solving  a  bilevel  optimization  problem 
where  \\L(KX  +  H(K)r) ||  is  maximized  subject  to  the 
constraint  X  G  C(r,K,a)  and  bisections  are  performed 
on  the  value  of  a  so  that  the  maximum  value  is  driven  to 

I.  As  we  demonstrate  in  this  paper,  in  special  cases  this 
computation  can  be  greatly  simplified. 


Fig.  5:  The  trajectory  of  r  and  the  debris. 
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Suppose  that  the  thrust  constraints  are  prescribed  in  terms 
of  polyhedral  norm  bounds,  specifically,  as 

ej  (KX  +  Hr)  <  umax,  i  =  1,2,  •••  ,m,  (17) 

where  are  the  vertices  of  the  unit  norm  polytope  and  umax 
is  the  norm  bound.  The  infinity  norm,  for  instance,  has  m  = 
6,  and 


1 

"  -1  " 

0 

ei  = 

0 

62  = 

0 

63  = 

1 
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0 

0 

0 

0 

0 

e4  = 

-1 

65  = 
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66  = 

0 

0 

1 

-1 

In  the  case  of  non-poly hedral  norm  bounds,  such  as  the  2- 
norm,  an  approximation  by  a  polyhedral  norm  bound  can  be 
employed. 

Then,  the  determination  of  the  thrust  limit  on  the  growth 
distance  is  based  on  solving  a  sequence  of  the  following 
optimization  problems  for  i  =  1,  •  •  •  ,  n, 

ej  (KX  +  Hr)  — »  max,  (19) 

subject  to 

\{X  -  Xe(r))T  P(X  -  Xe(r))  <  c.  (20) 

If  the  value  of  c  is  found  for  which  the  solutions  of  (19)- 
(20),  X*,  satisfy  ma Xi{ej(KX*  +  Hr)}  =  umax,  then 
pu(r,K)  =  y/c.  The  problem  (19)-(20)  can  be  solved  by 
diagonalizing  P,  using  an  orthogonal  matrix,  V, 

P  =  VtAV,  A  =  diag[Af ,  •  •  •  ,  A§],  A*  >  0. 

By  defining,  z  =  X  —  Xe(r),  and  £  so  that 

it  follows  that 

zTPz  =  (TA-iVPVTA~i( 

=  CTC. 


The  problem  (19)-(20)  can  now  be  re-written  as 


hj  £  +  ej  Yr  — >  max, 

(21) 

subject  to 

where 

2CTC<c, 

hj  =  ejKVT  A"  3. 

(22) 

The  solution  to  the  constrained  maximization  problem 
(21)-(22)  of  maximizing  the  inner-product  of  two  vectors 
over  a  unit  2-norm  ball  is  given  by 

<#  (23) 

INI 

where  1 1  •  1 1  denotes  the  vector  2 -norm.  The  maximum  value 
of  the  objective  function  is 

\\hi\\y/2c+  ejTr. 


Consequently,  to  satisfy  (17),  we  use  the  following  relations: 


c  =  0  if  for  any  i ,  umax  <  ejYr. 

1  ,umax  —  ei  Yr  2  , 

c  =  mm  -  ( - — — rp - )  otherwise. 

i  2  1 1  hi  1 1 


(24) 


Thus  the  problem  of  finding  the  thrust  limit  on  the  growth 
distance,  when  polyhedral  norm  bounds  on  thrust  are  em¬ 
ployed,  has  an  explicit  solution  given  by  (24). 

We  note  that  the  condition  umax  >  max^ {ejYr}  is 
satisfied  if  the  available  thrust  can  maintain  the  equilibrium 
Xe(r)  in  steady-state.  We  also  note  that,  based  on  the  form 
of  T,  c  is  independent  of  ry,  the  in-track  component  of  the 
equilibrium  in  the  virtual  net.  Hence  the  computations  of 
pu(r,  K)  need  to  be  only  performed  with  ry  =  0. 

In  the  case  when  the  spacecraft  does  not  have  independent 
thrusters  in  x,  y  and  z  directions,  the  2-norm  thrust  limit 
is  more  practical.  Unfortunately,  maximizing  a  quadratic 
function  in  (19)  subject  to  (20)  is,  in  general,  a  non-convex 
problem.  In  this  case,  the  2-norm  bound  can  be  approximated 
by  a  polyhedral  norm  bound  (17),  with  the  vertices 
selected  on  the  unit  2-norm  ball  in  P3.  We  note  that  higher 
accuracy  of  this  approximation  requires  a  higher  number  of 
vertices  in  (17),  which  thus,  complicates  (24). 

Finally,  we  note  that  in  the  case  the  Au’s  are  treated  as 
control  inputs,  the  thrust  limit  on  the  growth  distance  is 
induced  by  the  available  Av.  Computing  the  thrust  limit  on 
the  growth  distance  in  this  case  is  completely  analogous  to 
computing  it  in  the  case  when  the  control  input  is  the  thrust 
force  or  thrust  acceleration. 


I.  Connectivity  Graph  and  Graph  Search 

We  now  introduce  a  notion  of  connectivity  between  two 
vertices  of  the  virtual  net,  G  M  and  rj  G  M.  The  vertex 
Vi  is  connected  to  the  vertex  rj  if  there  exists  a  gain  K  G  /C 
such  that 

Xe(ri)  e  intC(rj,K,p*(rj,K,z)),  (25) 

where  int  denotes  the  interior  of  a  set.  The  connectivity 
implies  that  a  spacecraft  located  close  to  an  equilibrium 
corresponding  to  r*  can  transition  to  an  equilibrium  Xe(rj) 
by  using  limited  thrust  and  avoiding  collision  with  the  debris. 
We  note  that  if  r*  is  connected  to  rj  this  does  not  imply  that, 
in  turn,  rj  is  connected  to  r^.  We  also  note  that  connectivity 
depends  on  the  existence  of  an  appropriate  control  gain  from 
the  set  of  gains  /C  but  the  condition  (25)  does  not  need  to 
hold  for  all  gains. 

The  on-line  motion  planning  with  debris  avoidance  is  per¬ 
formed  according  to  the  following  procedure  (for  simplicity, 
described  here  for  the  case  of  a  single  debris): 

Step  1:  Determine  the  debris  location  and  shape  (i.e.,  2 
and  Q). 

Step  2:  By  using  fast  growth  distance  computations, 
determine  thrust  limited  growth  distance  based 
on  (12),  with  pg  computed  online  and  pu  pre¬ 
computed  off-line. 

Step  3:  Construct  a  graph  connectivity  matrix  between 
all  Vi ,  rj  G  M.  In  the  graph  connectivity  matrix,  if 
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two  vertices  are  not  connected,  the  corresponding 
matrix  element  is  zero;  if  they  are  connected  the 
corresponding  matrix  element  is  1.  In  parallel,  build 
the  control  gain  selectivity  matrix,  which  identifies 
the  index  of  the  highest  preference  gain  K  for 
which  ri  and  rj  are  connected.  This  gain  will 
be  applied  if  the  edge  connecting  r*  and  rj  is 
traversed. 

Step  4:  Perform  graph  search  to  determine  a  sequence 
of  connected  vertices  r[k]  G  M  and  control  gains 
K[k\  G  /C,  k  =  1,  •  •  *  ,  Zp,  such  that  r[  1]  satisfies 
the  initial  constraints,  r[lp\  satisfies  the  final  con¬ 
straints,  and  the  path  length  lp  is  minimized. 

Per  the  above  algorithm,  a  graph  search  is  utilized  to 
determine  the  minimum  number  of  equilibrium  hops  around 
a  piece  of  debris.  After  the  path  has  been  determined  as 
a  sequence  of  the  set-points  and  the  corresponding  control 
gains,  the  execution  of  the  path  proceeds  by  checking  if 
the  current  state,  X(t)  is  in  the  safe  positively  invariant  set 
corresponding  to  the  next  reference  r+  and  next  control  gain 
K+  in  the  sequence;  if  it  is,  then  the  controller  switches  to 
this  reference  and  control  gain: 

X(t)  G  C(r+,  K+,  p*(r+,  K+,  z))  r(t)  =  r+  K(t)  =  K+ 

IV.  Simulation  Results 

Simulations  are  now  provided  that  illustrate  our  debris 
avoidance  approach.  We  consider  a  nominal  circular  orbit  of 
850  km  and  discretize  the  HCW  equations  with  a  sampling 
period,  AT,  of  30  seconds.  We  construct  an  approximately 
2  km  cubed  virtual  net.  We  let  R  =  2  x  107/3  and  Q  = 
diag{  100, 100, 100, 107, 107, 107). 

An  ellipsoidal  set  over-bounding  a  piece  of  debris  is 
0(zi,Qi)  centered  at  z\  =  [0.3  0.4  0.5]T  km  and  with 
Q i  =  IOO/3.  In  this  example,  we  use  the  fast  distance 
computation  technique  of  [1]  based  on  bisections  applied 
to  (15)  to  determine  the  growth  distance  to  the  debris  from 
each  node  in  the  net.  The  spacecraft’s  initial  condition  is 
2/(0)  =  2/e(r0),  where  r0  =  [0.45  0  2.25]T.  The  target 
equilibrium  node  is  2/e(0).  Finally,  we  impose  a  maximum 
thrust  constraint  of  10  N  in  each  axis.  Dijkstra’s  algorithm  is 
used  to  find  the  shortest  cost  path  from  initial  node  to  final 
node. 

Figure  6  shows  the  path  the  spacecraft  takes  under  closed- 
loop  control  in  order  to  avoid  the  debris.  The  spacecraft  is 
able  to  complete  the  desired  maneuver  well  within  maximum 
thrust  constraints  while  successfully  avoiding  the  debris. 

In  Figure  7  we  rerun  the  above  simulation  for  a  grid  of 
initial  conditions.  The  maneuvers  shown  in  Figure  7  clearly 
demonstrate  the  initial  conditions  for  which  the  maneuver 
path  is  perturbed  from  that  which  the  spacecraft  would  have 
taken  had  there  been  no  debris. 

Next,  we  add  a  second  piece  of  debris,  0(2:2,  Q2),  centered 
at  Z2  =  [—0.3  0.4  0.5]t  and  with  Q2  =  IOO/3.  In  calculating 
the  growth  distance,  we  take  the  minimum  distance  to  each  of 
0(zi,Qi),  i  =  1,2  .  Figure  8  shows  the  path  the  spacecraft 
takes  under  closed-loop  control  in  order  to  avoid  the  two 
debris. 


(a)  Debris  Avoidance  Path. 


(b)  Norm  of  Thrust  Profile. 


Fig.  6:  (a)  Debris  avoidance  path  for  a  single  debris.  The 
green  x  marks  the  initial  node.  The  blue  x  marks  the  final 
node.  The  red  ellipsoid  represents  the  debris.  The  blue  line  is 
the  path  the  spacecraft  takes  in  order  to  avoid  the  debris.  The 
blue  ellipsoids  represent  the  invariant  sets  along  the  path,  (b) 
The  time  history  of  thrust  magnitude. 


Finally,  we  consider  the  case  of  a  non- stationary  debris 
where  we  treat  its  motion  as  the  union  of  static  debris  along 
the  path  as  described  in  (10).  A  union  of  ellipsoidal  sets 
over-bound  the  debris’  motion,  Utid  Ofe,  Qi),  where  Z{ 
are  generated  by  sampling  the  relative  motion  of  the  debris 
with  the  initial  condition  [0  0.5  0  0  0.0006  0]T,  and  with 
Qi  =  2OO/3,  i  =  1  •  •  -nd. 

The  spacecraft’s  initial  condition  is  2/(0)  =  2/e(ro), 
where  ro  =  [0  1  0]T.  The  target  equilibrium  node  is  Xe(0). 
We  impose  a  maximum  thrust  constraint  of  10  N  in  each 
axis.  Dijkstra’s  algorithm  is  used  to  find  the  shortest  cost 
path  from  initial  node  to  final  node.  The  simulation  results 
in  Figure  9  demonstrate  the  the  spacecraft  is  able  to  avoid 
the  debris  path  by  hopping  over  it. 

V.  Conclusion 

This  paper  described  a  technique  for  spacecraft  maneuver 
planning  that  uses  positively-invariant  sets  in  order  to  avoid 
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Fig.  7:  Debris  avoidance  paths  for  many  initial  conditions. 
Each  green  x  marks  an  intial  condition.  The  blue  x  marks 
the  final  node.  The  red  ellipsoid  represents  the  debris.  The 
blue  lines  are  the  paths  that  the  spacecraft  takes  from  each 
initial  condition  in  order  to  avoid  the  debris.  We  do  not  show 
the  invariant  set  ellipsoids  for  visual  clarity. 


collisions  with  debris,  while  adhering  to  specified  thrust 
limits.  The  approach  is  based  on  hopping  between  neigh¬ 
borhoods  of  equilibria  in  a  virtual  net,  and  maintaining  the 
spacecraft  trajectory  within  a  tube  formed  by  safe  positively  - 
invariant  sets.  For  the  case  where  thrust  limits  can  be 
specified  as  polyhedral  norm  bounds,  we  have  shown  that  the 
thrust  limit  on  the  growth  distance  can  be  easily  computed; 
it  is,  in  fact,  feasible  to  perform  these  computations  onboard 
a  spacecraft  in  order  to  account  for  thruster  failure  or 
degradation. 
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The  green  x  marks  the  initial  node.  The  blue  x  marks  the 
final  node.  The  red  ellipsoids  represent  the  debris  path.  The 
blue  line  is  the  path  the  spacecraft  takes  in  order  to  avoid  the 
debris.  The  blue  ellipsoids  represent  the  invariant  sets  along 
the  path,  (b)  The  time  history  of  thrust  magnitude. 
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